#ifndef __PROTOCOL_H__
#define __PROTOCOL_H__

#include "common.h"
#include "crc.h"  /* Include EASML CRC Library */

/* Protocol Configuration */
#define PROTOCOL_HEADER_SIZE 4u
#define PROTOCOL_MAX_DATA_SIZE 64u
#define PROTOCOL_MAX_PACKET_SIZE (PROTOCOL_HEADER_SIZE + PROTOCOL_MAX_DATA_SIZE + 2u)
#define PROTOCOL_SYNC_BYTE1 0xAAu
#define PROTOCOL_SYNC_BYTE2 0x55u
#define PROTOCOL_TIMEOUT_MS 1000u

/* Command Codes */
typedef enum {
    CMD_SERVO_MOVE = 0x01u,
    CMD_SERVO_MOVE_ALL = 0x02u,
    CMD_SERVO_STOP = 0x03u,
    CMD_KINEMATICS_MOVE = 0x10u,
    CMD_KINEMATICS_GET_POS = 0x11u,
    CMD_SENSOR_READ = 0x20u,
    CMD_SENSOR_READ_ALL = 0x21u,
    CMD_SYSTEM_STATUS = 0x30u,
    CMD_SYSTEM_RESET = 0x31u,
    CMD_CALIBRATE = 0x40u,
    CMD_SAVE_CONFIG = 0x41u,
    CMD_LOAD_CONFIG = 0x42u,
    CMD_RESPONSE_OK = 0x80u,
    CMD_RESPONSE_ERROR = 0x81u
} Protocol_CmdCode_Type;

/* Status Codes */
typedef enum {
    PROTOCOL_STATUS_OK = 0u,
    PROTOCOL_STATUS_ERROR = 1u,
    PROTOCOL_STATUS_CRC_ERROR = 2u,
    PROTOCOL_STATUS_TIMEOUT = 3u,
    PROTOCOL_STATUS_INVALID_CMD = 4u,
    PROTOCOL_STATUS_BUFFER_FULL = 5u,
    PROTOCOL_STATUS_PARSE_ERROR = 6u
} Protocol_Status_Type;

/* Packet Header Structure */
typedef struct {
    uint8 SyncByte1;       /* 0xAA */
    uint8 SyncByte2;       /* 0x55 */
    uint8 Command;         /* Command code */
    uint8 DataLength;      /* Data payload length */
} Protocol_Header_Type;

/* Communication Packet Structure */
typedef struct {
    Protocol_Header_Type Header;
    uint8 Data[PROTOCOL_MAX_DATA_SIZE];
    uint16 Crc;            /* CRC16 checksum using EASML CRC */
    uint8 TotalLength;     /* Total packet length */
} Protocol_Packet_Type;

/* Command Data Structures */

/* Servo Move Command */
typedef struct {
    uint8 ServoIndex;      /* Servo index 0-5 */
    sint16 Angle;          /* Angle in 0.1 degrees */
    uint16 Time;           /* Movement time in ms */
} Protocol_ServoMoveCmd_Type;

/* Servo Move All Command */
typedef struct {
    sint16 Angles[6];      /* All servo angles in 0.1 degrees */
    uint16 Time;           /* Movement time in ms */
} Protocol_ServoMoveAllCmd_Type;

/* Kinematics Move Command */
typedef struct {
    sint16 X;              /* X position in 0.1 mm */
    sint16 Y;              /* Y position in 0.1 mm */
    sint16 Z;              /* Z position in 0.1 mm */
    sint16 Alpha;          /* Pitch angle in 0.1 degrees */
    uint16 Time;           /* Movement time in ms */
} Protocol_KinematicsMoveCmd_Type;

/* Sensor Read Response */
typedef struct {
    uint8 SensorType;      /* Sensor type */
    sint32 Value;          /* Sensor value (scaled) */
    uint8 Status;          /* Sensor status */
} Protocol_SensorReadResp_Type;

/* System Status Response */
typedef struct {
    uint8 SystemState;     /* Overall system state */
    uint8 ServoStatus[6];  /* Individual servo status */
    sint16 CurrentAngles[6]; /* Current servo angles */
    uint16 BatteryVoltage; /* Battery voltage in mV */
    uint16 Temperature;    /* Temperature in 0.1°C */
    uint32 Uptime;         /* System uptime in seconds */
} Protocol_SystemStatusResp_Type;

/* Parser State Machine */
typedef enum {
    PARSE_STATE_WAIT_SYNC1 = 0u,
    PARSE_STATE_WAIT_SYNC2 = 1u,
    PARSE_STATE_WAIT_CMD = 2u,
    PARSE_STATE_WAIT_LENGTH = 3u,
    PARSE_STATE_WAIT_DATA = 4u,
    PARSE_STATE_WAIT_CRC = 5u,
    PARSE_STATE_PROCESS = 6u
} Protocol_ParseState_Type;

/* Communication Buffer Structure */
typedef struct {
    uint8 Buffer[PROTOCOL_MAX_PACKET_SIZE * 2];  /* Double buffer */
    uint16 WriteIndex;
    uint16 ReadIndex;
    uint16 DataAvailable;
    boolean PacketReady;
} Protocol_CommBuffer_Type;

/* Protocol Context Structure */
typedef struct {
    Protocol_CommBuffer_Type RxBuffer;
    Protocol_CommBuffer_Type TxBuffer;
    Protocol_Packet_Type CurrentPacket;
    Protocol_ParseState_Type ParseState;
    uint16 ExpectedBytes;
    uint16 ReceivedBytes;
    uint32 LastRxTime;
    Protocol_Status_Type Status;
    
    /* Statistics */
    uint32 PacketsReceived;
    uint32 PacketsSent;
    uint32 CrcErrors;
    uint32 TimeoutErrors;
} Protocol_Context_Type;

/* Function Declarations */

/* Initialization */
void Protocol_Init(Protocol_Context_Type *Context_pst);

/* Packet Building */
Protocol_Status_Type Protocol_BuildPacket(Protocol_Packet_Type *Packet_pst,
                                         Protocol_CmdCode_Type Command_e,
                                         const uint8 *Data_cpu8, uint8 DataLength_u8);

/* CRC Functions (using EASML CRC library) */
uint16 Protocol_CalculateCrc(const uint8 *Data_cpu8, uint16 Length_u16);
boolean Protocol_VerifyCrc(const Protocol_Packet_Type *Packet_cpst);

/* Packet Parsing */
Protocol_Status_Type Protocol_ReceiveByte(Protocol_Context_Type *Context_pst,
                                         uint8 ReceivedByte_u8, uint32 CurrentTime_u32);

Protocol_Status_Type Protocol_ParsePacket(Protocol_Context_Type *Context_pst,
                                         Protocol_Packet_Type *Packet_pst);

/* Command Processing */
Protocol_Status_Type Protocol_ProcessCommand(const Protocol_Packet_Type *Packet_cpst,
                                            Protocol_Packet_Type *Response_pst);

/* Response Building */
Protocol_Status_Type Protocol_BuildResponse(Protocol_Packet_Type *Response_pst,
                                           Protocol_CmdCode_Type ResponseCode_e,
                                           const void *ResponseData_cpv, uint8 DataLength_u8);

Protocol_Status_Type Protocol_BuildErrorResponse(Protocol_Packet_Type *Response_pst,
                                                Protocol_Status_Type ErrorCode_e);

/* Buffer Management */
Protocol_Status_Type Protocol_BufferWrite(Protocol_CommBuffer_Type *Buffer_pst,
                                         const uint8 *Data_cpu8, uint16 Length_u16);

Protocol_Status_Type Protocol_BufferRead(Protocol_CommBuffer_Type *Buffer_pst,
                                        uint8 *Data_pu8, uint16 Length_u16);

uint16 Protocol_BufferAvailable(const Protocol_CommBuffer_Type *Buffer_cpst);
void Protocol_BufferReset(Protocol_CommBuffer_Type *Buffer_pst);

/* Transmission */
Protocol_Status_Type Protocol_SendPacket(Protocol_Context_Type *Context_pst,
                                        const Protocol_Packet_Type *Packet_cpst);

Protocol_Status_Type Protocol_GetTxData(Protocol_Context_Type *Context_pst,
                                       uint8 *Data_pu8, uint16 *Length_pu16);

/* Data Conversion */
sint16 Protocol_FloatToFixed(float32 Value_f32, uint8 FracBits_u8);
float32 Protocol_FixedToFloat(sint16 Value_s16, uint8 FracBits_u8);

/* Utility Functions */
void Protocol_PackU16(uint8 *Buffer_pu8, uint16 Value_u16);
uint16 Protocol_UnpackU16(const uint8 *Buffer_cpu8);
void Protocol_PackS16(uint8 *Buffer_pu8, sint16 Value_s16);
sint16 Protocol_UnpackS16(const uint8 *Buffer_cpu8);
void Protocol_PackU32(uint8 *Buffer_pu8, uint32 Value_u32);
uint32 Protocol_UnpackU32(const uint8 *Buffer_cpu8);

/* Statistics */
void Protocol_GetStats(const Protocol_Context_Type *Context_cpst,
                      uint32 *PacketsRx_pu32, uint32 *PacketsTx_pu32,
                      uint32 *CrcErrors_pu32, uint32 *TimeoutErrors_pu32);

void Protocol_ResetStats(Protocol_Context_Type *Context_pst);

#endif /* __PROTOCOL_H__ */
